package crtl;

import java.util.HashMap;
import java.util.Map;
import java.util.Properties;


/**
 *
 * @author benjamin 
 */
public class BangBangLoveLightCtrl extends BangBangCtrl {

	private final int _finishedThreshold = 150;
	private final int _slowDownThreshold = 400;

	private boolean _finished = false;
	
	public BangBangLoveLightCtrl() {
		super();

		System.out.println("BangBang - Love Light Controller initialized");
	}
	
	@Override
	protected boolean finished() {
		return _finished;
	}

	@Override
	protected Speed navigate() {
		double[] s = readLightSensors();
		s = calcAverage(s);
		
		double min = Double.MAX_VALUE;
		Direction d = Direction.FORWARD;
		HashMap<Direction, Double> values = convertSensorValues(s);
		
		// Find minimal sensor value
		for (Map.Entry<Direction, Double> entry : values.entrySet()) {
			if (entry.getValue() < min) {
				d = entry.getKey();
				min = entry.getValue();
			}
		}

		double intensity = 1;
		if (min < _slowDownThreshold) {
			// Slow down speed
			intensity = (min - _finishedThreshold) / _slowDownThreshold;
			
		} else if (min < _finishedThreshold) {
			// Stop in front of the light
			intensity = 0;
			_finished = true;
		}
		
		return calcSpeed(d, intensity);
	}

}
